/*
 * Copyright (c) 2009, Hamish Morgan. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the University of Sussex nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
package locusts.server.behaviours;

import java.util.Arrays;
import java.util.Comparator;
import javax.vecmath.Vector2d;
import locusts.common.entities.Actor;

public class Accumulate extends AbstractBehaviourList {

    private double limit;

    public Accumulate(double weight, double limit) {
        super(weight);
        this.limit = limit;
    }

    @Override
    public Vector2d getSteering(Actor boid) {
        Vector2d[] forces = getForces(boid);
        Vector2d steering = accumulate(forces);
        steering.scale((double) getWeight());
        return steering;
    }

    /**
     * Combine various vectors commands using an accumulator. Works by taking
     * the vectors  in order of magnitude. They are added to the output
     * value until the limit is reached.
     */
    private final Vector2d accumulate(final Vector2d[] vecs) {
        if (vecs == null || vecs.length == 0)
            throw new IllegalArgumentException();

        final Vector2d out = new Vector2d();
        final Integer[] ordering = new Integer[vecs.length];
        
        final double[] lengthSqrd = new double[vecs.length];
        for (int i = 0; i < vecs.length; i++) {
            ordering[i] = i;
            lengthSqrd[i] = vecs[i].lengthSquared();
        }

        Arrays.sort(ordering, new Comparator<Integer>() {

            public int compare(Integer i, Integer j) {
                final double diff = lengthSqrd[i] -
                        lengthSqrd[j];
                return diff < 0 ? -1 : diff > 0 ? 1 : 0;
            }
        });

        final double limSqrd = limit * limit;

        double outLen = 0;
        int i = vecs.length - 1;

        while (i >= 0 && outLen < limSqrd) {
            final int j = ordering[i];
            if(lengthSqrd[j] == 0) {
                i--;
                continue;
            }
            final double remaining = limSqrd - outLen;
            double scale = (i + 1d) / vecs.length;
            final double nextLen = lengthSqrd[j] * scale;

            if (nextLen > remaining)
                scale *= remaining / nextLen;
                
            out.scaleAdd((double)scale, vecs[j], out);
            outLen += nextLen * scale;
            i--;
            
        }

        return out;
    }

  

    @Override
    public boolean equals(Object obj) {
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        final Accumulate other = (Accumulate) obj;
        if (this.limit != other.limit) return false;
        return super.equals(obj);
    }

    @Override
    public int hashCode() {
        int hash = super.hashCode();
        hash =
                43 * hash +
                (int) (Double.doubleToLongBits(this.limit) ^
                (Double.doubleToLongBits(this.limit) >>> 32));
        return hash;
    }



}
